#include "move_task.h"

#include <QString>
#include <algorithm>
#include <chrono>

#include "application/map/area/area.h"
#include "application/map/area/area_manager.h"
#include "kernel/agv/agv_entity.h"

namespace application {
namespace task {

MoveTask::MoveTask(
    const std::string& targetStation,
    int floor,
    std::shared_ptr<vehicle::VehicleService> vehicleService,
    std::shared_ptr<dispatcher::DispatcherService> dispatcherService,
    std::shared_ptr<MapApplicationService> mapService,
    std::shared_ptr<kernel::ILogger> logger,
    const std::string& operation)
    : SubTaskBase(logger)
    , m_targetStation(targetStation)
    , m_floor(floor)
    , m_vehicleService(std::move(vehicleService))
    , m_dispatcherService(std::move(dispatcherService))
    , m_mapService(std::move(mapService))
    , m_operation(operation)
{
}

kernel::task::TaskStage MoveTask::init(TaskContext& ctx)
{
    if (!m_vehicleService) {
        return finishWithError(ctx, "VehicleService not available");
    }

    if (!m_dispatcherService) {
        return finishWithError(ctx, "DispatcherService not available");
    }

    if (m_targetStation.empty()) {
        return finishWithError(ctx, "Target station is empty");
    }

    if (!ctx.agv) {
        return finishWithError(ctx, "No AGV assigned to move task");
    }

    if (!ctx.agv->isOnline()) {
        logDebug("AGV offline, waiting before issuing move task");
        return kernel::task::TaskStage::Init;
    }

    if (checkCrossFloor(ctx)) {
        ctx.requestStatus(kernel::task::TaskChainStatus::Recalculation);
        return kernel::task::TaskStage::Init;
    }

    if (!acquireTargetArea(ctx)) {
        return kernel::task::TaskStage::Init;
    }

    if (ctx.agv->isCharging()) {
        m_vehicleService->stopCharging(ctx.agv->getNumber());
        ctx.requestStatus(kernel::task::TaskChainStatus::Suspend, DEFAULT_SUSPEND_TICKS);
        return kernel::task::TaskStage::Init;
    }

    if (!m_taskIssued) {
        m_lastStation = ctx.agv->getCurrentStation();
        m_lastNavStatus = ctx.agv->getNavStatus();
        m_lastProgressTime = std::chrono::steady_clock::now();
        logInfo("Requesting navigation for AGV " +
                std::to_string(ctx.agv->getNumber()) + " to station " +
                m_targetStation);

        bool success = m_dispatcherService->requestNavigation(
            ctx.agv,
            m_targetStation,
            m_floor,
            m_operation
        );

        if (!success) {
            return finishWithError(ctx, "Failed to request navigation");
        }

        m_taskIssued = true;
        m_waitingForDispatch = true;
        m_lastProgressTime = std::chrono::steady_clock::now();
        logInfo("Navigation request submitted successfully");
    }

    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage MoveTask::run(TaskContext& ctx)
{
    if (!ctx.agv) {
        return finishWithError(ctx, "AGV lost during task execution");
    }

    const auto now = std::chrono::steady_clock::now();
    bool progressMade = false;

    if (m_lastProgressTime == std::chrono::steady_clock::time_point{}) {
        m_lastProgressTime = now;
        if (ctx.agv) {
            m_lastStation = ctx.agv->getCurrentStation();
        }
    }

    auto navStatus = ctx.agv->getNavStatus();
    const bool statusChanged = (navStatus != m_lastNavStatus);
    if (statusChanged) {
        m_lastNavStatus = navStatus;
        m_lastProgressTime = now;
        clearTimeoutState(ctx);  // 状态切换视为有进展，重置超时判定
    }

    std::string currentStation = ctx.agv->getCurrentStation();
    if (!currentStation.empty() && currentStation != m_lastStation) {
        m_lastStation = currentStation;
        m_lastProgressTime = now;
        if (m_waitingForDispatch) {
            logDebug("Dispatcher started navigation, station changed to " + currentStation);
        }
        m_waitingForDispatch = false;
        progressMade = true;
    }

    const bool manualStop = (navStatus == kernel::agv::NavStatus::None &&
        m_taskIssued && !m_waitingForDispatch);

    if (navStatus == kernel::agv::NavStatus::Running ||
        navStatus == kernel::agv::NavStatus::Suspended ||
        navStatus == kernel::agv::NavStatus::Canceled ||
        manualStop) {
        if (navStatus == kernel::agv::NavStatus::Running && !statusChanged) {
            progressMade = true;
        }
        // Treat manual cancel as a neutral state; dispatcher may send new route
        if (navStatus == kernel::agv::NavStatus::Canceled) {
            logWarning("Navigation manually canceled for AGV " +
                std::to_string(ctx.agv->getNumber()) +
                ", waiting for dispatcher to resume");
        } else if (manualStop) {
            logWarning("Navigation manually stopped for AGV " +
                std::to_string(ctx.agv->getNumber()) +
                ", waiting for dispatcher to resume");
        }
        if (!manualStop) {
            m_waitingForDispatch = false;
        }
    } else if (navStatus == kernel::agv::NavStatus::Failed) {
        m_waitingForDispatch = false;
        logWarning("Navigation failed for AGV " +
                   std::to_string(ctx.agv->getNumber()) +
                   ", awaiting manual intervention");
        
        
    } else if (navStatus == kernel::agv::NavStatus::Completed) {
        m_waitingForDispatch = false;
        progressMade = true;
    }

    if (progressMade) {
        clearTimeoutState(ctx);
    }

    if (m_waitingForDispatch) {
        logDebug("Waiting for dispatcher to publish navigation task");
        return kernel::task::TaskStage::Running;
    }

    if (!m_areaOccupied) {
        acquireTargetArea(ctx);
    }

    if (ctx.agv->isCharging()) {
        ctx.requestStatus(kernel::task::TaskChainStatus::Suspend, DEFAULT_SUSPEND_TICKS);
        return kernel::task::TaskStage::Init;
    }

    const bool arrived = hasArrived(ctx);
    if (!arrived) {
        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - m_lastProgressTime).count();
        if (elapsed > NAVIGATION_PROGRESS_TIMEOUT_SEC) {
            const std::string message = "导航任务执行超时 AGV " +
                std::to_string(ctx.agv->getNumber());
            enterTimeoutState(ctx, message);
            return kernel::task::TaskStage::Running;
        }
    } else {
        clearTimeoutState(ctx);
    }

    if (arrived && isNavigationComplete(ctx)) {
        logInfo("AGV " + std::to_string(ctx.agv->getNumber()) +
                " arrived at station " + m_targetStation);
        return kernel::task::TaskStage::Cleanup;
    }

    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage MoveTask::cleanup(TaskContext& ctx)
{
    logDebug("Move task cleanup");
    clearTimeoutState(ctx);

    releaseOccupiedResources(ctx);

    m_waitingForDispatch = false;
    m_lastStation.clear();
    m_lastProgressTime = std::chrono::steady_clock::time_point{};
    m_lastNavStatus = kernel::agv::NavStatus::None;
    return kernel::task::TaskStage::Finished;
}

void MoveTask::enterTimeoutState(TaskContext& ctx, const std::string& message)
{
    ctx.setError(message);
    ctx.requestStatus(kernel::task::TaskChainStatus::Timeout);
    if (!m_timeoutNotified) {
        logWarning(message);
        m_timeoutNotified = true;
    }
}

void MoveTask::clearTimeoutState(TaskContext& ctx)
{
    if (!m_timeoutNotified) {
        return;
    }
    ctx.requestStatus(kernel::task::TaskChainStatus::Ok);
    ctx.clearError();
    m_timeoutNotified = false;
}

void MoveTask::notifyDispatch(const QList<QString>& segment)
{
    m_waitingForDispatch = false;
    m_lastProgressTime = std::chrono::steady_clock::now();
    if (!segment.isEmpty()) {
        m_lastStation = segment.front().toStdString();
        std::string info = "Dispatcher dispatched segment: " +
                           segment.front().toStdString() + " -> " +
                           segment.back().toStdString();
        logDebug(info);
    } else {
        logDebug("Dispatcher dispatched segment: <empty>");
    }
}

bool MoveTask::hasArrived(const TaskContext& ctx) const
{
    if (!ctx.agv) {
        return false;
    }

    std::string currentStation = ctx.agv->getCurrentStation();

    if (currentStation == m_targetStation) {
        return true;
    }

    if (m_targetStation.size() > 2 && m_targetStation.substr(0, 2) == "LM") {
        std::string targetWithoutPrefix = m_targetStation.substr(2);
        if (currentStation == targetWithoutPrefix) {
            return true;
        }
    }

    return false;
}

bool MoveTask::isNavigationComplete(const TaskContext& ctx) const
{
    if (!ctx.agv) {
        return false;
    }

    auto navStatus = ctx.agv->getNavStatus();
    return navStatus == kernel::agv::NavStatus::Completed ||
           navStatus == kernel::agv::NavStatus::Canceled ||
           navStatus == kernel::agv::NavStatus::None;
}

bool MoveTask::checkCrossFloor(TaskContext& ctx) const
{
    if (!ctx.agv || !m_mapService || m_floor <= 0) {
        return false;
    }

    QString mapName = m_mapService->getMapNameForFloor(m_floor);
    if (mapName.isEmpty()) {
        return false;
    }

    std::string targetMap = mapName.toStdString();
    if (ctx.agv->getCurrentMap() != targetMap) {
        return true;
    }

    return false;
}

bool MoveTask::acquireTargetArea(TaskContext& ctx)
{
    if (!m_mapService || m_floor < 0) {
        return true;
    }

    auto area = getTargetArea();
    if (!area) {
        return true;
    }

    if (ctx.agv && !area->isOccupiedBy(ctx.agv->getNumber()) && area->isOccupied()) {
        logDebug("Target area occupied by other AGV, waiting");
        return false;
    }

    if (ctx.agv && !m_areaOccupied) {
        const int agvNumber = ctx.agv->getNumber();
        bool acquired = true;
        const bool alreadyOwned = area->isOccupiedBy(agvNumber);

        if (m_dispatcherService) {
            if (!alreadyOwned) {
                acquired = m_dispatcherService->occupyArea(
                    m_targetStation,
                    agvNumber,
                    m_floor);
            }
        } else if (!alreadyOwned) {
            area->occupy(agvNumber);
        }

        if (!acquired) {
            logDebug("Waiting for third-party area confirmation");
            return false;
        }

        ctx.addOccupiedArea(area);
        m_areaOccupied = true;
        m_occupyingAgvNumber = agvNumber;
    }

    return true;
}

std::shared_ptr<area::Area> MoveTask::getTargetArea() const
{
    if (!m_mapService) {
        return nullptr;
    }
    auto areaMgr = m_mapService->getAreaManager(m_floor);
    if (!areaMgr) {
        return nullptr;
    }
    const QString areaId = QString::fromStdString(m_targetStation);
    if (!areaMgr->hasArea(areaId)) {
        return nullptr;
    }
    return areaMgr->getArea(areaId);
}

void MoveTask::releaseOccupiedResources(TaskContext& ctx)
{
    if (!m_areaOccupied) {
        return;
    }

    const int agvNumber = ctx.agv ? ctx.agv->getNumber() : m_occupyingAgvNumber;

    if (agvNumber >= 0) {
        if (m_dispatcherService) {
            m_dispatcherService->releaseArea(m_targetStation, agvNumber, m_floor);
        } else {
            if (auto area = getTargetArea()) {
                area->releaseOccupation(agvNumber);
            }
        }
    }

    auto& occupiedAreas = ctx.occupiedAreas;
    occupiedAreas.erase(std::remove_if(occupiedAreas.begin(), occupiedAreas.end(),
        [&](const std::weak_ptr<area::Area>& weakArea) {
            auto areaPtr = weakArea.lock();
            if (!areaPtr) {
                return true;
            }
            return areaPtr->getAreaId().toStdString() == m_targetStation;
        }),
        occupiedAreas.end());

    m_areaOccupied = false;
    m_occupyingAgvNumber = -1;
}

void MoveTask::resetNavigationRequest()
{
    m_taskIssued = false;
    m_waitingForDispatch = false;
    m_lastStation.clear();
    m_lastProgressTime = std::chrono::steady_clock::time_point{};
}

}  // namespace task
}  // namespace application
